/*
 * @FilePath: \MDK-ARMd:\Engineeringtemplate\RC2022\RC-Control-2022\R2\Task\control_task.c
 * @Description: 
 * @Author: ???
 * @version: 0.1
 * @Date: 2022-03-17 21:59:32
 * @copyright: Copyright (c) 2022 SPR
 */

#include "control_task.h"
#include "main.h"
#include "cmsis_os.h"
#include "stdio.h"
#include "bsp_usart.h"
#include "fifo.h"
#include "protocol.h"
#include "chassis_task.h"
#include "trigonometric.h"
#include "math.h"

// static void chassis_target_set(chassis_move_t *chassis_move_control_loop, RC_ctrl_t* rc_ctrl);
/**
 * @brief: 
 * @param {int16_t} vx
 * @param {int16_t} vy
 * @return {*}
 */
float set_motor_target_ang(int16_t vx, int16_t vy)
{
  if (vx == 0)
  {
    if (vy >= 0)
      return 0;
    else if (vy < 0)
      return 180;
  }
  float theta;
  theta = atan(fabs(vy / vx)) * 180 / PI;
  if (vx > 0)
  {
    if (vy >= 0)
      return 90-theta;
    else if (vy < 0)
    {
      theta += 90;
      return theta;
    }
  }
  else if (vx < 0)
  {
    if (vy >= 0)
    {
      theta += 270;
    }
    else if (vy < 0)
    {
      theta = 270 - theta;
    }
  }

  return theta;
}

int set_motor_target_vel(int16_t rx,int16_t ry)
{
  int16_t v =mySqrt((rx-1024)*(rx-1024)+(ry-1024)*(ry-1024));

  return 8*v;
}

void control_task(void const *pvParameters)
{
  vTaskDelay(200);
  while (1)
  {
    agv_move.chassis_steer.motor_chassis->angle_set = set_motor_target_ang(rc_ctrl.rc.ch[0], rc_ctrl.rc.ch[1]);
    agv_move.chassis_driver.motor_chassis->speed_set = set_motor_target_vel(rc_ctrl.rc.ch[0],rc_ctrl.rc.ch[1]);

        vTaskDelay(20);
  }
}

/**
 * @brief
 * @param[out]     chassis_move_set:"chassis_move"变量指针.
 * @retval         none
 */

// static void chassis_target_set(chassis_move_t *chassis_move_set, RC_ctrl_t* rc_ctrl)
//{
//	float wheel_speed[4]={0.0f,0.0f,0.0f,0.0f};
////	wheel_speed[0] = rc_ctrl->rc
//}

// static void chassis_vector_to_mecanum_wheel_speed(const fp32 vx_set, const fp32 vy_set, const fp32 wz_set, fp32 wheel_speed[4])
//{
//   // because the gimbal is in front of chassis, when chassis rotates, wheel 0 and wheel 1 should be slower and wheel 2 and wheel 3 should be faster
//   //
//   wheel_speed[0] = -vx_set - vy_set + (CHASSIS_WZ_SET_SCALE - 1.0f) * MOTOR_DISTANCE_TO_CENTER * wz_set;
//   wheel_speed[1] = vx_set - vy_set + (CHASSIS_WZ_SET_SCALE - 1.0f) * MOTOR_DISTANCE_TO_CENTER * wz_set;
//   wheel_speed[2] = vx_set + vy_set + (-CHASSIS_WZ_SET_SCALE - 1.0f) * MOTOR_DISTANCE_TO_CENTER * wz_set;
//   wheel_speed[3] = -vx_set + vy_set + (-CHASSIS_WZ_SET_SCALE - 1.0f) * MOTOR_DISTANCE_TO_CENTER * wz_set;
// }

// calculate the max speed in four wheels, limit the max speed
//
//  for (i = 0; i < 4; i++)
//  {
////    chassis_move_control_loop->motor_chassis[i].speed_set = wheel_speed[i];
//    temp = fabs(chassis_move_control_loop->motor_chassis[i].speed_set);
//    if (max_vector < temp)
//    {
//      max_vector = temp;
//    }
//  }
//  if (max_vector > MAX_WHEEL_SPEED)
//  {
//    vector_rate = MAX_WHEEL_SPEED / max_vector;
//    for (i = 0; i < 4; i++)
//    {
//      chassis_move_control_loop->motor_chassis[i].speed_set *= vector_rate;
//    }
//  }
